#pragma once
#ifndef PRM_H
#define PRM_H

#include "basic_typedef.h"
#include "Graph.h"
#include "Kd_tree_d.h"
#include "Sampler.h"
#include "CollisionDetector.h"

struct cmpPoint_2 {
    bool operator()(const Point_2& a, const Point_2& b) const {
        return a.id() > b.id();
    }
};

class Prm
{
public:
    Prm(Polygon_2 &robot_polygon, Obstacles &obstacles, Conf &start, Conf &target, Room &room);
    Prm(Polygon_2 &robot_polygon, Obstacles &obstacles, Conf &start, Conf &target, Room &room, CollisionDetector* collision_detector);
    ~Prm();

    void generate_roadmap(int number_vertices = 300, int k_nearest = 12, double eps = 1);
    void update_path(vector<Conf> &path);
    Conf generate_valid_sample();
    Conf generate_sample();
    Conf& get_neighbor(Conf& sample, Conf target);
    vector<Conf*> get_neighbors(Conf& sample);
    bool has_path();

private:
    Polygon_2                   &m_robot_polygon;
    Obstacles                   &m_obstacles;           // polygonal obstacles + scenario boundaries
    Conf                        &m_start, &m_target;
    Room                        &m_room;

    Graph<Conf, cmpPoint_2>  m_graph;                // Graph structure
    Kd_tree_2<Kernel>           m_kd_tree;              // Kd-tree for nearest neighbor search

    CollisionDetector*          m_collision_detector;
    Sampler                     m_sampler;
    bool                        m_sould_clear;

    vector<Conf>                m_vertices;             // Contains all the points in the configuration space
    bool                        m_first_time;
};


#endif